#!/usr/bin/env python
# -*- coding: utf-8 -*-

import envs
import time
import rospy
import actionlib

from npu_robot.msg import ArmSwingAction, ArmSwingGoal
from npu_robot.msg import ArmThetaAction, ArmThetaGoal


class ArmObject():
    def __init__(self):
        # 机械臂摇摆客户端
        self.arm_swing_client = actionlib.SimpleActionClient('xarm_controller/arm_swing', ArmSwingAction)

        # 机械臂夹爪旋转客户端
        self.arm_theta_client = actionlib.SimpleActionClient('xarm_controller/arm_theta', ArmThetaAction)

        # 机械臂摇摆
        self.goal_swing = ArmSwingGoal()

        # 机械臂夹爪旋转
        self.goal_theta = ArmThetaGoal()

        # 机械臂关节初始化
        self.joint_states = []

    def active_callback(self):
        """
        action激活后回调（一次）
        """
        rospy.logdebug('arm object action is actived')

    def done_callback(self, state, res):
        """
        action完成后回调（一次）
        """
        rospy.logdebug('arm object action is finished')
        rospy.logdebug('state: %s', state)
        rospy.logdebug('res: %s', res)

        self.joint_states = list(res.joint_states)

    def feedback_callback(self, feedback):
        """
        action执行过程中反馈的回调（多次）
        """
        rospy.logdebug('arm object action is feedbacking...')
        rospy.logdebug('feedback: %s', feedback)

    def arm_swing_send_goal(self, goal):
        """
        action请求（机械臂）
        """
        self.arm_swing_client.wait_for_server()

        self.arm_swing_client.send_goal(goal, done_cb=self.done_callback, active_cb=self.active_callback, feedback_cb=self.feedback_callback)
        self.arm_swing_client.wait_for_result(rospy.Duration.from_sec(300.0))

    def arm_swing(self):
        """
        机械臂左右摇摆
        """
        rospy.loginfo('[arm_object] arm swing...')

        self.goal_swing.id = 1
        self.arm_swing_send_goal(self.goal_swing)

        rospy.loginfo('[arm_object] arm swing OK')

    def arm_theta_send_goal(self, goal):
        """
        action请求（机械臂）
        """
        self.arm_theta_client.wait_for_server()

        self.arm_theta_client.send_goal(goal, done_cb=self.done_callback, active_cb=self.active_callback, feedback_cb=self.feedback_callback)
        self.arm_theta_client.wait_for_result(rospy.Duration.from_sec(300.0))

    def arm_theta(self):
        """
        机械臂夹爪旋转
        """
        rospy.loginfo('[arm_object] arm theta...')

        self.goal_theta.id = 1
        self.arm_theta_send_goal(self.goal_theta)
        time.sleep(1.5)

        rospy.loginfo('[arm_object] arm theta OK')

        return self.joint_states


if __name__ == '__main__':
    rospy.init_node('npu_robot_arm_object', anonymous=True, log_level=envs.log_level)
    arm_object = ArmObject()
    # arm_object.arm_swing()
    joint_states = arm_object.arm_theta()
    rospy.loginfo('joint_states: %s', joint_states)
